/*!
\file AlgAStar.cpp
\brief Implementation of AlgAStar Algorithm.
\ingroup Plugins
*/

#include"AlgAStar.h" 

AlgAStar::AlgAStar( Graph *g ):Algorithm(g){
	containerAdd();
	containerAdd();	
}

AlgAStar::~AlgAStar(){

}

void AlgAStar::run(){
	astar( graph()->node(0), graph()->node(graph()->numNodes()-1) );
}

void AlgAStar::astar( const Node &start, const Node &dest ){
	m_dest = &dest;
	addToOpenList(start);
	
	while( open().size() > 0 ){
		selectFromOpenList();
		const Node &n = *closed()[0]->node();
		Data *d = m_closedData[0];
		
		if( n == *m_dest ){
			printRoute( n, d );
			return;
		}
		
		QList<Edge> edges = graph()->edgesFrom(n);
		for( int i = 0; i < edges.size(); i++ ){
			const Node &other = otherNode(edges[i], n);
			
			if( !isInClosedList( other ) ){
				addToOpenList( other, n.identifier(), d->g + edges[i].label().toDouble() );
			}
		}
	}
}

void AlgAStar::printRoute( const Node &n, Data *d ){
	printf( "%s\n", QString("printRoute called for ").append(n.toString()).toAscii().data() );
	char str[1024];
	if( d->parentID == -1 )
		sprintf(str, "Node %d, %s\n", n.identifier(), n.label().toAscii().data());
	else{
		const Node &parent = graph()->nodeWithId(d->parentID);
		bool found = false;
		for( unsigned int i = 0; i < closed().size() && !found; i++ ){
			found = *closed()[i]->node() == parent;
			if(found)
				printRoute(*closed()[i]->node(), m_closedData[i]);
		}
		sprintf(str, "Node %d, %s\n", n.identifier(), n.label().toAscii().data() );
	}
	comment(str);
	highlight(n);
}

bool AlgAStar::isInClosedList( const Node &n ){
	for( unsigned int i = 0; i < closed().size(); i++ )
		if( *closed()[i]->node() == n )
			return true;
	return false;
}

Node AlgAStar::otherNode( const Edge &e, const Node &n ){
	if( e.from() == n )
		return e.to();
	else
		return e.from();
}

void AlgAStar::selectFromOpenList(){
	int pos = 0;
	int minF = open()[0]->dataDouble();
	for( int i = 1; i < open().size(); i++ ){
		if( open()[i]->dataDouble() < minF ){
			minF = open()[i]->dataDouble();
			pos = i;
		}
	}
	
	ContainerElement *e = open().takeAt(pos);
	Data *d = m_openData.takeAt(pos);
	
	comment( e->node()->toString().append(" at estimated distance ").append(QString().setNum(e->dataDouble())).append( " was selected from open list. Moved to closed list." ) );
	
	closed().push_front( e );
	m_closedData.push_front( d );
}

void AlgAStar::addToOpenList(const Node &node, int parentID, double g){
	double h = guessDistance(node);
	double f = g + h;
	bool shouldAdd = true;
	
	shouldAdd = true;
	for( int i = 0; i < open().size() && shouldAdd; i++ ){
		if( *open()[i]->node() == node ){
			if( m_openData[i]->g > g ){
				m_openData[i]->g = g;
				m_openData[i]->h = h;
				m_openData[i]->f = f;
				m_openData[i]->parentID = parentID;
				comment( node.toString().append(" received a better parent.") );
				open()[i]->setData(f);
			}
			shouldAdd = false;
		}
	}
	if(shouldAdd){
		comment( node.toString().append(" at estimated distance ").append(QString().setNum(f)).append(" was added to open list.") );
		open().push_back( (new ContainerElement)->setNode(node)->setData(f) );
		m_openData.push_back( new Data( g, h, f, parentID ) );
	}
}

double AlgAStar::guessDistance( const Node &node ){
	Coordinates a(node.label().toAscii().data());
	Coordinates b(m_dest->label().toAscii().data());
	double ret = sqrt( (a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y) );
	printf( "Distance guessed from node %d to end, (%lf, %lf) to (%lf, %lf): %lf\n", node.identifier(), a.x, a.y, b.x, b.y, ret );
	printf( "Labels used: %s and %s\n", node.label().toAscii().data(), m_dest->label().toAscii().data() );
	return ret;
}
